


#include "qe_def.h"
#include "qe_pack.h"
#include "qe_array.h"
#include "qe_memory.h"
#include "qe_service.h"
#include "qe_assert.h"
#include "qe_log.h"



QELOG_DOMAIN("qe-pack");



qe_u8 qe_checksum_u8(char *buf, unsigned int len)
{
    int i;
    qe_u8 sum = 0;

    for (i=0; i<len; i++) {
        sum += buf[i];
    }

    return sum;
}

qe_u32 qe_checksum_u32(char *buf, unsigned int len)
{
    int i;
    qe_u32 sum = 0;

    for (i=0; i<len; i++) {
        sum += buf[i];
    }

    return sum;
}

/**
 * @brief BinaryPackage parse function
 * 
 * @param[in] hdr: header
 * @param[in] sectab_bytes: section table bytes pointer
 * @param[out] sections: sections array
 * 
 * @return qe_ret
 */
qe_ret qe_binpkg_parse(qe_binpkg_header *hdr, qe_u8 *sectab_bytes, qe_array *sections)
{
    int i;
    qe_u8 *pos;
    qe_u32 tag;
    qe_u32 size;
    qe_u32 offset;
    qe_binpkg_section section;

    if (!hdr || !sectab_bytes || !sections) {
        return qe_err_param;
    }

    qe_hexdump_info(sectab_bytes, 36);

    pos = sectab_bytes;

    for (i=0; i<hdr->num_sections; i++) {
        qe_unpack_dwrd(tag, pos);
        qe_unpack_dwrd(offset, pos);
        qe_unpack_dwrd(size, pos);
        section.tag = qe_htonl(tag);
        section.size = qe_htonl(size);
        section.offset = qe_htonl(offset);
        qe_array_append(sections, &section, 1);
    }

    return qe_ok;
}

/**
 * @brief BinaryPackage get section table size
 * 
 * @param[in] hdr: header struct
 * 
 * @return table size
 */
qe_size qe_binpkg_get_sectab_size(qe_binpkg_header *hdr)
{
    hdr->magic = qe_htonl(hdr->magic);
    hdr->num_sections = qe_htonl(hdr->num_sections);
    hdr->sectab_offset = qe_htonl(hdr->sectab_offset);
    return (hdr->num_sections * sizeof(qe_binpkg_section));
}

qe_int qe_pack_buffer_write(qe_pack_buffer *buf, const char* data, qe_size len)
{
    qe_assert(buf || len == 0);
    if (!buf) 
        return 0;

    if (buf->alloc - buf->size < len) {
        void* tmp;
        qe_size nsize = (buf->alloc) ?
                buf->alloc * 2 : 1024;

        while (nsize < buf->size + len) {
            qe_size tmp_nsize = nsize * 2;
            if (tmp_nsize <= nsize) {
                nsize = buf->size + len;
                break;
            }
            nsize = tmp_nsize;
        }

        tmp = qe_realloc(buf->data, nsize);
        if(!tmp) { return -1; }

        buf->data = (char*)tmp;
        buf->alloc = nsize;
    }

    qe_memcpy(buf->data + buf->size, (char *)data, len);
    buf->size += len;

    return 0;
}

void qe_pack_buffer_init(qe_pack_buffer *buf)
{
    qe_memset(buf, 0, sizeof(qe_pack_buffer));
}

qe_pack_buffer *qe_pack_buffer_new(void)
{
    return (qe_pack_buffer *)qe_malloc(sizeof(qe_pack_buffer));
}

void qe_pack_buffer_destroy(qe_pack_buffer *buf)
{
    if (buf->data) {
        buf->size = 0;
        buf->alloc = 0;
        qe_free(buf->data);
        buf->data = QE_NULL;
    }
}

#define SLIP_END            0xC0
#define SLIP_ESC            0xDB
#define SLIP_ESC_END        0xDC
#define SLIP_ESC_ESC        0xDD

/**
 * @brief Slip decode in buffer to out buffer
 * 
 * @param[in] in: input buffer
 * @param[in] in_size: input buffer bytes
 * @param[out] out: output decoded buffer
 * @param[out] out_size: output decoded buffer size
 * 
 * @return <0:error >=0:decode size
 */
qe_int qe_slip_decode(qe_u8 *in, qe_size in_size, 
    qe_u8 *out, qe_size out_size)
{
    qe_u8 *pos_in;
    qe_u8 *pos_out;
    qe_bool find_end = qe_false;

    if (!in || !in_size || !out || !out_size) {
        qe_error("invalid params");
        return -1;
    }

    pos_in  = in;
    pos_out = out;

    while (in_size--) {

        switch (*pos_in) {

        /* END means begin or end */
        case SLIP_END:
            if (find_end)
                return (pos_out - out);
            else
                find_end = qe_true;
            pos_in++;
            break;

        case SLIP_ESC:
            pos_in++;
            if (*pos_in == SLIP_ESC_END)
                *pos_out++ = SLIP_END;
            else
                *pos_out++ = SLIP_ESC;
            pos_in++;
            break;
        
        default:
            if (find_end)
                *pos_out++ = *pos_in++;
            else
                pos_in++;
            break;
        }

        if (pos_out > (out + out_size)) {
            qe_error("decode out of range");
            return -1;
        }
    }

    return -1;
}

/**
 * @brief Slip encode
 * @param[in] in: input buffer
 * @param[in] in_size: input buffer length
 * @param[out] out: output buffer
 * @param[out] out_size: output buffer length
 * 
 * @return <0:error >=0:encode size
 */
qe_int qe_slip_encode(qe_u8 *in, qe_size in_size, 
    qe_u8 *out, qe_size out_size)
{
    qe_u8 *pos_in;
    qe_u8 *pos_out;

    if (!in || !in_size || !out || !out_size) {
        qe_error("invalid params");
        return -1;
    }

    pos_in  = in;
    pos_out = out;

    /* prefix end */
    *pos_out++ = SLIP_END;

    while (in_size--) {
        
        switch (*pos_in) {

        case SLIP_END:
            *pos_out++ = SLIP_ESC;
            *pos_out++ = SLIP_ESC_END;
            pos_in++;
            break;

        case SLIP_ESC:
            *pos_out++ = SLIP_ESC;
            *pos_out++ = SLIP_ESC_ESC;
            pos_in++;
            break;

        default:
            *pos_out++ = *pos_in++;
            break;
        }
        
        if (pos_out > (out + out_size)) {
            qe_error("out buffer too small");
            return -1;
        }
    }

    /* postfix end */
    if (pos_out > (out + out_size)) {
        qe_error("out buffer too small");
        return -1;
    }
    *pos_out++ = SLIP_END;
    return (pos_out - out);
}

/**
 * @brief Find a complete SLIP packet from the buffer
 * @param[in] buf: buffer to find
 * @param[in] size: buffer length
 * @param[out] len: find SLIP packet length
 * 
 * @return <0:error >=0:find size
 */
qe_int qe_slip_find(qe_u8 *buf, qe_size size, qe_size *len)
{
    qe_u8 *pos, *begin;
    qe_bool find_end = qe_false;

    if (!buf || !size)
        return -1;

    pos = buf;
    while (size--) {
        if (*pos == SLIP_END) {
            if (find_end) {
                *len = pos - begin + 1;
                return (begin - buf);
            } else {
                find_end = qe_true;
                begin = pos;
            }
        }
        pos++;
    }

    return -1;
}

/**
 * @brief Find SLIP begin from the buffer
 * @param[in] buf: buffer to find
 * @param[in] size: buffer length
 * 
 * @return <0:error >=0:SLIP begin offset
 */
qe_int qe_slip_begin(qe_u8 *buf, qe_size size)
{
    qe_u8 *pos;

    if (!buf || !size)
        return -1;

    pos = buf;
    while (size--) {
        if (*pos == SLIP_END) {
            return (pos - buf);
        }
        pos++;
    }

    return -1;
}

qe_ret qe_slip_pack(qe_pack_buffer *buf, qe_u8 *in, qe_size in_size)
{
    qe_u8 *pos_in;
    qe_u8 byte;

    if (!buf || !in || !in_size) {
        qe_error("invalid params");
        return qe_err_param;
    }

    pos_in = in;

    /* prefix end */
    byte = SLIP_END;
    qe_pack_buffer_write(buf, &byte, 1);

    while (in_size--) {
        
        switch (*pos_in) {

        case SLIP_END:
            byte = SLIP_ESC;
            qe_pack_buffer_write(buf, &byte, 1);
            byte = SLIP_ESC_END;
            qe_pack_buffer_write(buf, &byte, 1);
            pos_in++;
            break;

        case SLIP_ESC:
            byte = SLIP_ESC;
            qe_pack_buffer_write(buf, &byte, 1);
            byte = SLIP_ESC_ESC;
            qe_pack_buffer_write(buf, &byte, 1);
            pos_in++;
            break;

        default:
            byte = *pos_in++;
            qe_pack_buffer_write(buf, &byte, 1);
            break;
        }
    }

    byte = SLIP_END;
    qe_pack_buffer_write(buf, &byte, 1);

    return qe_ok;
}

qe_ret qe_slip_unpack(qe_pack_buffer *buf, qe_u8 *in, qe_size in_size)
{
    qe_u8 *pos_in;
    qe_u8 byte;
    qe_bool find_end = qe_false;

    if (!in || !in_size || !buf) {
        qe_error("invalid params");
        return qe_err_param;
    }

    pos_in  = in;

    while (in_size--) {

        switch (*pos_in) {

        /* END means begin or end */
        case SLIP_END:
            if (find_end)
                return qe_ok;
            else
                find_end = qe_true;
            pos_in++;
            break;

        case SLIP_ESC:
            pos_in++;
            if (*pos_in == SLIP_ESC_END) {
                byte = SLIP_END;
                qe_pack_buffer_write(buf, &byte, 1);
            } else {
                byte = SLIP_ESC;
                qe_pack_buffer_write(buf, &byte, 1);
            }
            pos_in++;
            break;
        
        default:
            if (find_end) {
                byte = *pos_in++;;
                qe_pack_buffer_write(buf, &byte, 1);
            } else {
                pos_in++;
            }
            break;
        }
    }

    return qe_err_param;
}

/**
 * @subsection Serial General Package(SERGPK) 
 */

#define SERGP_LENGTH_BE         (0)
#define SERGP_TLV_SIZE          (4) /* type(1) length(2) sum(1) */
#define SERGP_PKG_SIZE(len)     ((len) + SERGP_TLV_SIZE + 2)

/**
 * @brief SERGP package into a buffer
 * @param[in] type: TLV type
 * @param[in] data: package data
 * @param[in] len: TLV length
 * @param[out] out: SERGP buffer
 * @param[out] size: SERGP buffer size
 * 
 * @return <0:error >=0: package length
 */
qe_int qe_sergp_pack(qe_u8 *type, qe_u8 *data, qe_size len, 
    qe_u8 *out, qe_size size)
{
    qe_u8  sum;
    qe_u8 *pos;
    qe_u8 *temp_buf;
    qe_u16 length;
    qe_int n;

    if (!data || !len || !out || !size) {
        qe_error("invalid params");
        return -1;
    }

    if (SERGP_PKG_SIZE(len) > size) {
        return -1;
    }

    temp_buf = qe_malloc(len + SERGP_TLV_SIZE);
    qe_assert(temp_buf != QE_NULL);

    pos = temp_buf;
    qe_pack_byte(type, pos);
#if (SERGP_LENGTH_BE == 1)
    length = qe_htons(len);
#else
    length = len;
#endif
    qe_pack_word(length, pos);
    qe_memcpy(pos, data, len);
    pos += len;
    sum = qe_checksum_u8((char *)temp_buf, pos-temp_buf);
    qe_pack_byte(sum, pos);

    n = qe_slip_encode(temp_buf, pos-temp_buf, out, size);
    qe_free(temp_buf);
    if (n < 0) {
        qe_error("encode error");
        return -1;
    } else {
        return n;
    }
}

/**
 * @brief SERGP unpack from buffer into a <qe_sergp>
 * @param[in] buf: SERGP packet
 * @param[in] len: SERGP length
 * @param[out] s: unpack struct
 * 
 * @return <qe_ret>
 * 
 * @note This function need you alloc memory for <qe_sergp> before.
 * You can create by qe_sergp_new()
 */
qe_ret qe_sergp_unpack(qe_u8 *buf, qe_size len, qe_sergp *s)
{
    qe_u8 sum;
    qe_int decode_bytes;
    qe_uint vlen;
    qe_uint bufsz;
    qe_gbuf *sbuf;

    if (!buf || !len || !s || !s->buf) {
        qe_error("invalid params");
        return qe_err_param;
    }

    sbuf = s->buf;
    if (sbuf->size < (len - 2)) {
        qe_error("buffer size too small, bufsz:%d len:%d", 
            sbuf->size, len);
        return qe_err_param;
    }

    decode_bytes = qe_slip_decode(buf, len, 
        (qe_u8 *)qe_gbuf_pos(sbuf), qe_gbuf_size(sbuf));
    if (decode_bytes <= 0) {
        //qe_error("slip decode error");
        return qe_err_common;
    }
    qe_gbuf_data_size(sbuf) = decode_bytes;
    qe_hexdump_debug(qe_gbuf_pos(sbuf), qe_gbuf_data_size(sbuf));

#if (SERGP_LENGTH_BE == 1)
    vlen = qe_gbuf_index(sbuf, 1) << 8 | qe_gbuf_index(sbuf, 2);
#else
    vlen = qe_gbuf_index(sbuf, 2) << 8 | qe_gbuf_index(sbuf, 1);
#endif

    if ((vlen + 4) != decode_bytes) {
        // qe_error("length error tlv len:%d slip decode len:%d", 
        //     vlen, decode_bytes);
        return qe_err_common;
    }

    s->tlv.s    = (qe_u8 *)qe_gbuf_pos(sbuf);
    s->tlv.type = qe_gbuf_index(sbuf, 0);
    s->tlv.len  = vlen;
    s->tlv.v    = (qe_u8 *)&qe_gbuf_index(sbuf, 3);
    s->sum      = qe_gbuf_index(sbuf, s->tlv.len+3);

    sum = qe_checksum_u8((char *)s->tlv.s, s->tlv.len+3);
    if (sum != s->sum) {
        //qe_error("sum not same");
        return qe_err_common;
    }

    return qe_ok;
}

/**
 * @brief Create a <qe_sergp> with give buffer size
 * @param[in] size: buffer size
 * 
 * @return: <qe_sergp>
 */
qe_sergp *qe_sergp_new(qe_size size)
{
    qe_sergp *s = qe_malloc(sizeof(qe_sergp));
    qe_assert(s != QE_NULL);
    qe_memset(s, 0x0, sizeof(qe_sergp));
    s->buf = qe_gbuf_new(size);
    qe_assert(s->buf != QE_NULL);
    return s;
}

/**
 * @brief Destroy a <qe_sergp>
 */
void qe_sergp_destroy(qe_sergp *s)
{
    if (s->buf)
        qe_free(s->buf);
    qe_free(s);
}

/**
 * @brief Unpack SERGP dump to <qe_sergp>
 * @param[in] buf: SERGP buffer
 * @param[in] len: SERGP length
 * 
 * @return <qe_sergp>
 */
qe_sergp *qe_sergp_unpack_dup(qe_u8 *buf, qe_size len)
{
    qe_u8 sum;
    qe_size bufsz;
    qe_int decode_bytes;
    qe_uint vlen;
    qe_sergp *s;
    qe_gbuf *sbuf;

    if (!buf || !len) {
        qe_error("invalid params");
        return QE_NULL;
    }

    if (len < 6) {
        qe_error("too small");
        return QE_NULL;
    }

    s = qe_new(qe_sergp);
    qe_assert(s != QE_NULL);

    bufsz = len * 2;
    s->buf = qe_gbuf_new(bufsz);
    qe_assert(s->buf != QE_NULL);
    sbuf = s->buf;

    decode_bytes = qe_slip_decode(buf, len, 
        (qe_u8 *)qe_gbuf_pos(sbuf), qe_gbuf_size(sbuf));
    if (decode_bytes <= 0) {
        qe_error("slip decode error");
        qe_sergp_destroy(s);
        return QE_NULL;
    }
    qe_gbuf_data_size(sbuf) = decode_bytes;

#if (SERGP_LENGTH_BE == 1)
    vlen = qe_gbuf_index(sbuf, 1) << 8 | qe_gbuf_index(sbuf, 2);
#else
    vlen = qe_gbuf_index(sbuf, 2) << 8 | qe_gbuf_index(sbuf, 1);
#endif

    if ((vlen + 4) != decode_bytes) {
        qe_error("length error");
        qe_sergp_destroy(s);
        return QE_NULL;
    }

    s->tlv.s    = (qe_u8 *)qe_gbuf_pos(sbuf);
    s->tlv.type = qe_gbuf_index(sbuf, 0);
    s->tlv.len  = vlen;
    s->tlv.v    = (qe_u8 *)&qe_gbuf_index(sbuf, 3);
    s->sum      = qe_gbuf_index(sbuf, s->tlv.len+3);

    sum = qe_checksum_u8((char *)s->tlv.s, s->tlv.len+3);
    if (sum != s->sum) {
        qe_error("sum not same");
        qe_sergp_destroy(s);
        return QE_NULL;
    }

    return s;
}
